/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * GenericVirtualMotor : This class allow differential drive motor simulation.
 **/
package org.hh.locodo;

import org.hh.locodo.MotorInterface;


/**
 * 
 * @author bilock
 *
 * Simulate a motor by incrementing an virtual encoder value according to the motor speed 
 */
public class GenericVirtualMotor implements MotorInterface {

  private double fDesiredSpeed;
  private double fSpeed;
  private double fEncVal;
  private double fMaxAcc;
  
  @SuppressWarnings("unused")
	private double fMaxSpeed;

  
	public GenericVirtualMotor() {
	  fSpeed = 0.f;
	  fEncVal = 0.f;
	  fMaxAcc = 0.07f; // encoder ticks per cycle
	  fDesiredSpeed = 0.f;
	  fMaxSpeed = 0.5f;
	}

	
	@Override
	public int getEncoderValue() {
		return (int) fEncVal;
	}

	
	@Override
	public void setSpeed(double speed) {
		fDesiredSpeed = speed;
	}

	
	@Override
	public void update() {
	  if( Math.abs(fDesiredSpeed - fSpeed) < fMaxAcc )
	    fSpeed = fDesiredSpeed;
	  else
	    if( fDesiredSpeed > fSpeed )
	      fSpeed += fMaxAcc;
	    else
	      fSpeed -= fMaxAcc;
	  
	  // Warning do not simulate saturation speed like this
	  // The angle control is not effective at max speed...
	  //if( fSpeed > fMaxSpeed ) fSpeed = fMaxSpeed; 
	  //if( fSpeed < -fMaxSpeed ) fSpeed = -fMaxSpeed; 
	  fEncVal += 100. * fSpeed;
	}
}
